﻿using System;
using WiMo.Games.Physics;
using WiMo.Games.Drawables;

namespace WiMo.Games.MathLib.Objects
{
    public class Spring : Calculus.ODE, ISpring
    {
        public float Mass { get; set; }
        public float MU { get; set; }
        public float K { get; set; }
        public float X0 { get; set; }

        public IBody Object { get; set; }

        public Spring(IBody obj, float mu, float k)
            : base(2)
        {
            Object = obj;
            this.Mass = Object.Mass;
            this.MU = mu;
            this.K = k;
            S = 0;

            X0 = obj.Position.Y;

            Q[0] = 0.0f;
            Q[1] = this.X0;

            _released = null;
        }

        public float Time { get { return S; } }

        public override float[] GetRightHandSide(float s, float[] q, float[] deltaQ, float ds, float scale)
        {
            float[] dq = new float[4];
            float[] newQ = new float[4];

            for (int i = 0; i < 2; ++i)
            {
                newQ[i] = q[i] + scale * deltaQ[i];
            }

            dq[0] = -ds * (MU * newQ[0] + K * newQ[1]) / Mass;
            dq[1] = ds * (newQ[0]);

            return dq;
        }

        private GameTime _released;
        private PointF _originalPosition;

        public void Release(GameTime gt)
        {
            Q[0] = 0.0f;
            Q[1] = X0;
            _released = gt.Clone();
            _originalPosition = Object.Position.Clone();
        }

        public void Update(GameTime gt)
        {
            if (_released != null)
            {                
                Calculus.ODESolver.RungeKutta(this, Convert.ToSingle(gt.ElapsedGameTime.TotalSeconds));
                Object.Position = new PointF(Object.Position.X, (int)X + _originalPosition.Y);

                if (Math.Abs(Vx) < 0.01f)
                    _released = null;
            }
        }

        public float Vx
        {
            get { return Q[0]; }
        }

        public float X
        {
            get
            {
                return Q[1];
            }

        }
    }
}
